﻿using System;
using System.IO;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using DeepLearning;

public class SIGGRAPH_Asia_2019 : NeuralAnimation
{
    public bool ShowBiDirectional = true;
    public bool ShowRoot = true;
    public bool ShowGoal = true;
    public bool ShowCurrent = true;
    public bool ShowPhase = false;
    public bool ShowContacts = false;
    public bool ShowEnvironment = true;
    public bool ShowInteraction = true;
    public bool ShowGUI = true;

    public Controller Controller;
    private TimeSeries TimeSeries;
    private TimeSeries.Root RootSeries;
    private TimeSeries.Style StyleSeries;
    private TimeSeries.Goal GoalSeries;
    private TimeSeries.Contact ContactSeries;
    private TimeSeries.Phase PhaseSeries;
    private CylinderMap Environment;
    private CuboidMap Geometry;

    private Vector3[] PosePrediction;
    private Matrix4x4[] RootPrediction;
    private Matrix4x4[] GoalPrediction;

    public Vector3[] getPosePrediction()
    {
        return PosePrediction;
    }

    public Matrix4x4[] getRootPrediction()
    {
        return RootPrediction;
    }

    public Matrix4x4[] getGoalPrediction()
    {
        return GoalPrediction;
    }

    // simulate action signals
    private float[] Signals = new float[6];

    private float[] SignalIdle = {1f, 0f, 0f, 0f, 0f, 0f};
    private float[] SignalWalk = {0f, 1f, 0f, 0f, 0f, 0f};
    private float[] SignalRun = {0f, 0f, 1f, 0f, 0f, 0f};
    private float[] SignalCarry = {0f, 0f, 0f, 1f, 0f, 0f};
    private float[] SignalOpen = {0f, 0f, 0f, 0f, 1f, 0f};
    private float[] SignalSit = {0f, 0f, 0f, 0f, 0f, 1f};
    private float[] SignalCW = {0f, 1f, 0f, 1f, 0f, 0f};
    private float[] SignalCI = {1f, 0f, 0f, 1f, 0f, 0f};

    private float UserControl = 0f;
    private float NetworkControl = 0f;

    private float InteractionSmoothing = 0.9f;

    private bool IsInteracting = false;

    // To load bvh and parse into humanbones
    //public BRTDriver animDriver;
    public BVHDriver animDriver;
    public ProcessText dealText;


    private UltimateIK.Model RightFootIK, LeftFootIK;

    public Controller GetController()
    {
        return Controller;
    }

    public TimeSeries GetTimeSeries()
    {
        return TimeSeries;
    }

    private void SetSignal(byte ASC)
    {
        // Set signals according to text input
        Signals = new float[6];

        switch (ASC)
        {
            case (byte) ActionType.Idle:
                Signals = SignalIdle;
                break;
            case (byte) ActionType.Walk:
                Signals = SignalWalk;
                break;
            case (byte) ActionType.Turn:
                Signals = SignalWalk;
                break;
            case (byte) ActionType.Run:
                Signals = SignalRun;
                break;
            case (byte) ActionType.Carry:
                Signals = SignalCarry;
                break;
            case (byte) ActionType.Open:
                Signals = SignalOpen;
                break;
            case (byte) ActionType.Sit:
                Signals = SignalSit;
                break;
        }

        if (ASC == (byte) ActionType.Carry)
        {
            if (dealText.action.CarryToPlusIdle == true)
                Signals = SignalCI;
            else if (dealText.action.CarryToPlusWalk == true)
                Signals = SignalCW;
            else if (dealText.action.CarryToIdle == true)
                Signals = SignalIdle;
        }
    }


    /*****************************************************************************/
    // style motion with objects
    private Dictionary<string, Transform> animBones;
    private bool isCapPut = false;
    private Transform cap, microphone, phone, bottle, sw;

    private void putCap()
    {
        Transform right_finger = animBones["RightIndexDistal"];
        Transform head = animBones["Head"];

        if (cap.position.y >= head.position.y)
            isCapPut = true;

        if (isCapPut)
        {
            cap.position = head.position + new Vector3(0f, 0.18f, 0f) + head.forward.normalized * 0.1f;
            //cap.position = head.position;
            cap.forward = head.forward;
        }
        else
        {
            cap.position = right_finger.position;
            cap.forward = right_finger.forward;
        }
    }

    private void singSongs()
    {
        Transform right_hand = animBones["RightHand"].transform;

        microphone.position = right_hand.position + new Vector3(-0.05f, 0.025f, 0.05f);
        microphone.rotation = right_hand.rotation * Quaternion.Euler(new Vector3(309.3f, 108.6f, 336.3f));
        //microphone.rotation = Quaternion.Euler(new Vector3(285.60f, 325.76f, 39.35f));

        //Quaternion delta = Quaternion.Inverse(right_hand.rotation) * microphone.rotation;
        //Debug.Log(delta.eulerAngles);
    }

    private void text_walk()
    {
        Transform right_hand = animBones["RightHand"].transform;

        phone.position = right_hand.position + new Vector3(-0.06f, 0.06f, -0.04f);
        phone.forward = right_hand.forward;

        //Debug.Log(right_hand.position);
    }

    private void sit_phone()
    {
        Transform right_hand = animBones["RightHand"].transform;

        phone.position = right_hand.position + new Vector3(-0.1f, -0.01f, -0.01f);
        phone.rotation = right_hand.rotation * Quaternion.Euler(new Vector3(10.0f, 332.6f, 33f));

        // phone.rotation = Quaternion.Euler(new Vector3(10.508f,217.03f, 7.329f));

        // Quaternion delta = Quaternion.Inverse(right_hand.rotation) * phone.rotation;
        // Debug.Log(delta.eulerAngles);


        //Debug.Log(right_hand.position);
    }

    private void drink_water()
    {
        Debug.Log("BOTTLE");
        Transform right_hand = animBones["RightHand"].transform;

        bottle.position = right_hand.position + new Vector3(0.0651f, -0.1500f, 0.07f);
        bottle.rotation = right_hand.rotation * Quaternion.Euler(new Vector3(358.4f, 250.1f, 90.1f));

        // bottle.rotation = Quaternion.Euler(new Vector3(306.3881f, 103.5781f, 260.2806f));

        // Quaternion delta = Quaternion.Inverse(right_hand.rotation) * bottle.rotation;
        // Debug.Log(delta.eulerAngles);
    }

    private void eat_sw()
    {
        Debug.Log("SANDWICH");
        Transform right_finger = animBones["RightIndexDistal"];

        sw.position = right_finger.position;
        sw.forward = right_finger.forward;
    }

    private void styleMotion()
    {
        switch (dealText.action.style)
        {
            case "putly":
                if (dealText.action.hasStyleObj && dealText.action.styleObjs[0].modelName.Equals("cap"))
                    putCap();
                break;
            case "singly":
                if (dealText.action.hasStyleObj && dealText.action.styleObjs[0].modelName.Equals("microphone"))
                    singSongs();
                break;
            case "textly":
                if (dealText.action.hasStyleObj && dealText.action.styleObjs[0].modelName.Equals("phone"))
                    text_walk();
                break;
            case "phonely":
                if (dealText.action.hasStyleObj && dealText.action.styleObjs[0].modelName.Equals("phone"))
                    sit_phone();
                break;
            case "drinkly":
                if (dealText.action.hasStyleObj && dealText.action.styleObjs[0].modelName.Equals("bottle"))
                    drink_water();
                break;
            case "dinnerly":
                if (dealText.action.hasStyleObj && dealText.action.styleObjs[0].modelName.Equals("sandwich"))
                    eat_sw();
                break;
        }
    }

    /***********************************************************************************/

    protected override void Setup()
    {
        foreach (string o_name in styleObjects)
        {
            GameObject obj = GameObject.Find(o_name);
            if (obj)
                NameToObject.Add(o_name, obj);
        }

        // Set controller signals
        Controller = new Controller();
        Controller.Signal idle = Controller.AddSignal("Idle");
        idle.Default = true;
        idle.Velocity = 0f;
        idle.AddKey(KeyCode.W, false);
        idle.AddKey(KeyCode.A, false);
        idle.AddKey(KeyCode.S, false);
        idle.AddKey(KeyCode.D, false);
        idle.AddKey(KeyCode.Q, false);
        idle.AddKey(KeyCode.E, false);
        idle.AddKey(KeyCode.V, true);
        idle.UserControl = 0.25f;
        idle.NetworkControl = 0.1f;

        Controller.Signal walk = Controller.AddSignal("Walk");
        walk.AddKey(KeyCode.W, true);
        walk.AddKey(KeyCode.A, true);
        walk.AddKey(KeyCode.S, true);
        walk.AddKey(KeyCode.D, true);
        walk.AddKey(KeyCode.Q, true);
        walk.AddKey(KeyCode.E, true);
        walk.AddKey(KeyCode.LeftShift, false);
        walk.AddKey(KeyCode.C, false);
        walk.Velocity = 1f;
        walk.UserControl = 0.25f;
        walk.NetworkControl = 0.25f;

        Controller.Signal run = Controller.AddSignal("Run");
        run.AddKey(KeyCode.LeftShift, true);
        run.Velocity = 3f;
        run.UserControl = 0.25f;
        run.NetworkControl = 0.25f;

        Controller.Signal carry = Controller.AddSignal("Carry");
        carry.AddKey(KeyCode.V, true);
        carry.Velocity = 0f;
        carry.UserControl = 0.1f;
        carry.NetworkControl = 0f;

        Controller.Signal open = Controller.AddSignal("Open");
        open.AddKey(KeyCode.F, true);
        open.Velocity = 0f;
        open.UserControl = 0.1f;
        open.NetworkControl = 0f;

        Controller.Signal sit = Controller.AddSignal("Sit");
        sit.AddKey(KeyCode.C, true);
        sit.Velocity = 0f;
        sit.UserControl = 0.25f;
        sit.NetworkControl = 0f;

        Environment = new CylinderMap(4f, 9, 9, true);
        Geometry = new CuboidMap(new Vector3Int(8, 8, 8));

        TimeSeries = new TimeSeries(6, 6, 1f, 1f, 5);
        RootSeries = new TimeSeries.Root(TimeSeries);
        StyleSeries = new TimeSeries.Style(TimeSeries, "Idle", "Walk", "Run", "Carry", "Open", "Sit", "Climb");
        GoalSeries = new TimeSeries.Goal(TimeSeries, Controller.GetSignalNames());
        ContactSeries =
            new TimeSeries.Contact(TimeSeries, "Hips", "RightWrist", "LeftWrist", "RightAnkle", "LeftAnkle");
        PhaseSeries = new TimeSeries.Phase(TimeSeries);
        for (int i = 0; i < TimeSeries.Samples.Length; i++)
        {
            RootSeries.Transformations[i] = transform.GetWorldMatrix(true);
            if (StyleSeries.Styles.Length > 0)
            {
                StyleSeries.Values[i][0] = 1f;
            }

            if (GoalSeries.Actions.Length > 0)
            {
                GoalSeries.Values[i][0] = 1f;
            }

            GoalSeries.Transformations[i] = transform.GetWorldMatrix(true);
            PhaseSeries.Values[i] = Mathf.Repeat((float) i / GetFramerate(), 1f);
            Geometry.Pivot = transform.GetWorldMatrix(true);
            Geometry.References[i] = transform.position;
        }

        PosePrediction = new Vector3[Actor.Bones.Length];
        RootPrediction = new Matrix4x4[7];
        GoalPrediction = new Matrix4x4[7];

        RightFootIK = UltimateIK.BuildModel(Actor.FindTransform("RightHip"),
            Actor.GetBoneTransforms(ContactSeries.Bones[3]));
        LeftFootIK = UltimateIK.BuildModel(Actor.FindTransform("LeftHip"),
            Actor.GetBoneTransforms(ContactSeries.Bones[4]));


        // **************************************************************
        // avoid not exist the file SGA.result
        if (!File.Exists(Params.interfaceFile))
        {
            TextWriter tw = new StreamWriter(Params.interfaceFile);
            tw.Close();
        }

        //Load the scene info and action command, defult is idle
        dealText.Init();

        // **************************************************************
    }


    protected override void Feed()
    {
        if (dealText.IsChangedOptim)
        {
            // 当输入文本后，需要更新场景模型
            // 动作改变，是通过优化部分改变“action”变量实现
            dealText.IsChangedOptim = false;
        }

        Controller.Update();
        dealText.action.angleTurnN = transform.localEulerAngles; // Get the forward rotation
        dealText.action.timeNow = Time.realtimeSinceStartup; // Get the present time

        // Get Root
        Matrix4x4 root = RootSeries.Transformations[TimeSeries.Pivot];

        // Control Cycle
        SetSignal(dealText.action.actASC);
        UserControl = Controller.PoolUserControl(Signals);
        NetworkControl = Controller.PoolNetworkControl(Signals);

        //Debug.Log(Signals[0] + " " + Signals[1] + " " + Signals[2] + " " + Signals[3] + " " + Signals[4] + " " + Signals[5]);

        // get into relevant action according to the signals
        if (IsInteracting)
        {
            //Do nothing because coroutines have control.
        }
        else if (dealText.action.actASC == (byte) ActionType.Carry)
        {
            StartCoroutine(Carry());
        }
        else if (dealText.action.actASC == (byte) ActionType.Open)
        {
            StartCoroutine(Open());
        }
        else if (dealText.action.actASC == (byte) ActionType.Sit)
        {
            StartCoroutine(Sit());
        }
        else
        {
            Default();
        }


        //**************************************************************

        // 如果正在优化过程中,并且需要重置到优化之前的状态
        // TODO 这里在 reset 时,rightHip leftHip 两个点的旋转有问题,以待后续解决
        if (dealText.IsOptim() && dealText.IsResetOptim())
        {
            //dealText.action = null;

            Debug.Log(dealText.motionAvatar.GetComponent<NavMove>().target);

            dealText.action = new ActionInfo();
            dealText.action.ResetStatus(transform.GetWorldMatrix()); // 输入 Anubis 的
            dealText.action.bothMotion = false;
            dealText.setSmodelStatus(dealText.bufferOld); // 处理模型的位置
            this._setCurStatus(dealText.bufferOld.snsmInfo); // 处理 Anubis的位置

            Debug.Log("==========>detect 1");

            dealText.setActResetCompleted(true);

            Debug.Log(dealText.motionAvatar.GetComponent<NavMove>().target);

            //float dist = (transform.position - dealText.bufferOld.nsm.GetPosition()).sqrMagnitude;
            //// Debug.Log(dist);
            //if (dist < 0.01f)
            //{
            //    dealText.setActResetCompleted(true);
            //}
            return;
        }

        //Input Bone Positions / Velocities
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            NeuralNetwork.Feed(Actor.Bones[i].Transform.position.GetRelativePositionTo(root));
            NeuralNetwork.Feed(Actor.Bones[i].Transform.forward.GetRelativeDirectionTo(root));
            NeuralNetwork.Feed(Actor.Bones[i].Transform.up.GetRelativeDirectionTo(root));
            NeuralNetwork.Feed(Actor.Bones[i].Velocity.GetRelativeDirectionTo(root));
        }

        //*************************************************************

        // float dist = (transform.position)
        if (!dealText.action.isCompleted && dealText.action.routesOptim != null &&
            dealText.action.idxOptim < dealText.action.routesOptim.Count)
        {
            // TimeSeries.KeyCount = 13 = 6+1+6
            // gap=RootSeries.GetPosition(sample.Index) - root.GetPosition()
            // gap 受到动作的影响，当走动时在1附近，静止时0附近，跑步时可能有其他值

            float gap = (RootSeries.GetPosition(7) - root.GetPosition()).sqrMagnitude;
            List<Matrix4x4> fpoints = dealText.getFutureRoutine(root, gap, dealText.action);

            for (int i = 0; i < TimeSeries.KeyCount; i++)
            {
                TimeSeries.Sample sample = TimeSeries.GetKey(i);

                if (i < 7)
                {
                    // past frame
                    NeuralNetwork.FeedXZ(RootSeries.GetPosition(sample.Index).GetRelativePositionTo(root));
                    NeuralNetwork.FeedXZ(RootSeries.GetDirection(sample.Index).GetRelativeDirectionTo(root));
                    NeuralNetwork.Feed(StyleSeries.Values[sample.Index]);
                }
                else
                {
                    // future frame
                    NeuralNetwork.FeedXZ(fpoints[i - 7].GetPosition().GetRelativePositionTo(root));
                    NeuralNetwork.FeedXZ(fpoints[i - 7].GetForward().GetRelativeDirectionTo(root));
                    NeuralNetwork.Feed(StyleSeries.Values[sample.Index]);
                }
            }
        }
        else if (!dealText.action.isCompleted && dealText.isNSMOptim && dealText.action.routesOptim == null &&
                 dealText.action.hasTarget)
        {
            Transform rfoot = dealText.haveRfoot();
            if (rfoot != null && Vector3.Distance(transform.position, dealText.action.routeLast.GetPosition()) < 0.1f)
            {
                Debug.Log("not null");
                Vector3 d = rfoot.GetWorldMatrix().GetForward();
                for (int i = 0; i < TimeSeries.KeyCount; i++)
                {
                    TimeSeries.Sample sample = TimeSeries.GetKey(i);
                    NeuralNetwork.FeedXZ(RootSeries.GetPosition(sample.Index).GetRelativePositionTo(root));
                    NeuralNetwork.FeedXZ(d.GetRelativeDirectionTo(root));
                    NeuralNetwork.Feed(StyleSeries.Values[sample.Index]);
                }
            }
            else
            {
                Debug.Log("null");
                for (int i = 0; i < TimeSeries.KeyCount; i++)
                {
                    TimeSeries.Sample sample = TimeSeries.GetKey(i);
                    NeuralNetwork.FeedXZ(RootSeries.GetPosition(sample.Index).GetRelativePositionTo(root));
                    NeuralNetwork.FeedXZ(RootSeries.GetDirection(sample.Index).GetRelativeDirectionTo(root));
                    NeuralNetwork.Feed(StyleSeries.Values[sample.Index]);
                }
            }
        }
        else
        {
            //Input Trajectory Positions / Directions / Velocities / Styles
            for (int i = 0; i < TimeSeries.KeyCount; i++)
            {
                TimeSeries.Sample sample = TimeSeries.GetKey(i);
                NeuralNetwork.FeedXZ(RootSeries.GetPosition(sample.Index).GetRelativePositionTo(root));
                NeuralNetwork.FeedXZ(RootSeries.GetDirection(sample.Index).GetRelativeDirectionTo(root));
                NeuralNetwork.Feed(StyleSeries.Values[sample.Index]);
            }
        }

        //*************************************************************

        //Input Goals
        for (int i = 0; i < TimeSeries.KeyCount; i++)
        {
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            NeuralNetwork.Feed(GoalSeries.Transformations[sample.Index].GetPosition().GetRelativePositionTo(root));
            NeuralNetwork.Feed(GoalSeries.Transformations[sample.Index].GetForward().GetRelativeDirectionTo(root));
            NeuralNetwork.Feed(GoalSeries.Values[sample.Index]);
        }

        //Input Environment
        Environment.Sense(root, LayerMask.GetMask("Default", "Interaction"));
        NeuralNetwork.Feed(Environment.Occupancies);

        //Input Geometry
        for (int i = 0; i < Geometry.Points.Length; i++)
        {
            NeuralNetwork.Feed(Geometry.References[i].GetRelativePositionTo(root));
            NeuralNetwork.Feed(Geometry.Occupancies[i]);
        }

        //Setup Gating Features
        NeuralNetwork.Feed(GenerateGating());
    }

    public void _setCurStatus(FeedNSMInfo status)
    {
        Controller = status.Controller;
        TimeSeries = status.TimeSeries;
        RootSeries = status.RootSeries;
        StyleSeries = status.StyleSeries;
        GoalSeries = status.GoalSeries;
        ContactSeries = status.ContactSeries;
        PhaseSeries = status.PhaseSeries;
        Environment = status.Environment;
        Geometry = status.Geometry;

        PosePrediction = status.PosePrediction;
        RootPrediction = status.RootPrediction;
        GoalPrediction = status.GoalPrediction;

        PosePrediction = new Vector3[status.PosePrediction.Length];
        RootPrediction = new Matrix4x4[status.RootPrediction.Length];
        GoalPrediction = new Matrix4x4[status.GoalPrediction.Length];

        for (int i = 0; i < status.PosePrediction.Length; i++)
        {
            PosePrediction[i] = status.PosePrediction[i];
        }

        for (int i = 0; i < status.RootPrediction.Length; i++)
        {
            RootPrediction[i] = status.RootPrediction[i];
        }

        for (int i = 0; i < status.GoalPrediction.Length; i++)
        {
            GoalPrediction[i] = status.GoalPrediction[i];
        }

        // status.rootInfo = new RootInfo();
        RootSeries.Transformations = new Matrix4x4[status.rootInfo.Transformations.Length];
        RootSeries.Velocities = new Vector3[status.rootInfo.Velocities.Length];

        for (int i = 0; i < RootSeries.Transformations.Length; i++)
        {
            RootSeries.Transformations[i] = status.rootInfo.Transformations[i];
        }

        for (int i = 0; i < RootSeries.Velocities.Length; i++)
        {
            RootSeries.Velocities[i] = status.rootInfo.Velocities[i];
        }


        // Debug.Log("RightHip====before========>"+Actor.GetBoneTransformation("RightHip").GetPosition());
        // Debug.Log("RightHip====before========>"+Actor.GetBoneTransformation("RightHip").GetRotation());


        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            Actor.Bones[i].Velocity = status.boneInfo[i].Velocity;
            Actor.Bones[i].Transform.position = status.boneInfo[i].TransformMat.GetPosition();
            // 以下两种写法,好像没啥变化
            // Actor.Bones[i].Transform.rotation = status.boneInfo[i].TransformMat.GetRotation();
            Quaternion trot = status.boneInfo[i].TransformMat.GetRotation();
            Actor.Bones[i].Transform.rotation = Quaternion.LookRotation(trot.GetForward(), trot.GetUp());


            Actor.Bones[i].Acceleration = status.boneInfo[i].Velocity;
            Actor.Bones[i].Force = status.boneInfo[i].Force;
            Actor.Bones[i].Index = status.boneInfo[i].Index;
            Actor.Bones[i].Parent = status.boneInfo[i].Parent;
            Actor.Bones[i].Childs = status.boneInfo[i].Childs;
            Actor.Bones[i].Length = status.boneInfo[i].Length;

            Actor.Bones[i].ApplyLength();
            // Debug.Log(status.boneInfo[0].TransformMat.GetPosition());
        }

        // Debug.Log("RightHip=====after=======>"+Actor.GetBoneTransformation("RightHip").GetPosition());
        // Debug.Log("RightHip=====after=======>"+Actor.GetBoneTransformation("RightHip").GetRotation());

        //Input Bone Positions / Velocities
        int k = 0;
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            NeuralNetwork.Feed(status.bones[k++]);
            NeuralNetwork.Feed(status.bones[k++]);
            NeuralNetwork.Feed(status.bones[k++]);
            NeuralNetwork.Feed(status.bones[k++]);
        }

        //Input Trajectory Positions / Directions / Velocities / Styles
        k = 0;
        for (int i = 0; i < TimeSeries.KeyCount; i++)
        {
            // TimeSeries.Sample sample = TimeSeries.GetKey(i);
            NeuralNetwork.FeedXZ(status.trajs[k++]);
            NeuralNetwork.FeedXZ(status.trajs[k++]);
            NeuralNetwork.Feed(status.styles[i]);
        }

        //Input Goals
        k = 0;
        for (int i = 0; i < TimeSeries.KeyCount; i++)
        {
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            NeuralNetwork.Feed(status.goals[k++]);
            NeuralNetwork.Feed(status.goals[k++]);
            NeuralNetwork.Feed(status.styGoals[i]);
        }

        //Input Environment
        // Environment.Sense(root, LayerMask.GetMask("Default", "Interaction"));
        NeuralNetwork.Feed(status.env);

        //Input Geometry
        for (int i = 0; i < Geometry.Points.Length; i++)
        {
            NeuralNetwork.Feed(status.geoms[i]);
            NeuralNetwork.Feed(status.geomsOcc[i]);
        }

        //Setup Gating Features
        NeuralNetwork.Feed(status.gats);
    }

    public FeedNSMInfo _getCurStatus()
    {
        FeedNSMInfo status = new FeedNSMInfo();
        status.Controller = Controller;
        status.TimeSeries = TimeSeries;
        status.RootSeries = RootSeries;
        status.StyleSeries = StyleSeries;
        status.GoalSeries = GoalSeries;
        status.ContactSeries = ContactSeries;
        status.PhaseSeries = PhaseSeries;
        status.Environment = Environment;
        status.Geometry = Geometry;

        status.PosePrediction = new Vector3[PosePrediction.Length];
        status.RootPrediction = new Matrix4x4[RootPrediction.Length];
        status.GoalPrediction = new Matrix4x4[GoalPrediction.Length];

        for (int i = 0; i < PosePrediction.Length; i++)
        {
            status.PosePrediction[i] = PosePrediction[i];
        }

        for (int i = 0; i < RootPrediction.Length; i++)
        {
            status.RootPrediction[i] = RootPrediction[i];
        }

        for (int i = 0; i < GoalPrediction.Length; i++)
        {
            status.GoalPrediction[i] = GoalPrediction[i];
        }

        // Get Root
        Matrix4x4 root = RootSeries.Transformations[TimeSeries.Pivot];

        status.rootInfo = new RootInfo();
        status.rootInfo.Transformations = new Matrix4x4[RootSeries.Transformations.Length];
        status.rootInfo.Velocities = new Vector3[RootSeries.Velocities.Length];

        for (int i = 0; i < RootSeries.Transformations.Length; i++)
        {
            status.rootInfo.Transformations[i] = RootSeries.Transformations[i];
        }

        for (int i = 0; i < RootSeries.Velocities.Length; i++)
        {
            status.rootInfo.Velocities[i] = RootSeries.Velocities[i];
        }

        // Debug.Log("RightHip============>"+Actor.GetBoneTransformation("RightHip").GetPosition());
        // Debug.Log("RightHip============>"+Actor.GetBoneTransformation("RightHip").GetRotation());

        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            BoneInfo tbinfo = new BoneInfo();
            tbinfo.TransformMat = Actor.Bones[i].Transform.GetWorldMatrix();
            tbinfo.Velocity = Actor.Bones[i].Velocity;
            tbinfo.Acceleration = Actor.Bones[i].Acceleration;
            tbinfo.Force = Actor.Bones[i].Force;
            tbinfo.Index = Actor.Bones[i].Index;
            tbinfo.Parent = Actor.Bones[i].Parent;
            tbinfo.Childs = Actor.Bones[i].Childs;
            tbinfo.Length = Actor.Bones[i].Length;

            status.boneInfo.Add(tbinfo);
        }

        //Input Bone Positions / Velocities
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            status.bones.Add(Actor.Bones[i].Transform.position.GetRelativePositionTo(root));
            status.bones.Add(Actor.Bones[i].Transform.forward.GetRelativeDirectionTo(root));
            status.bones.Add(Actor.Bones[i].Transform.up.GetRelativeDirectionTo(root));
            status.bones.Add(Actor.Bones[i].Velocity.GetRelativeDirectionTo(root));
        }

        //Input Trajectory Positions / Directions / Velocities / Styles
        for (int i = 0; i < TimeSeries.KeyCount; i++)
        {
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            status.trajs.Add(RootSeries.GetPosition(sample.Index).GetRelativePositionTo(root));
            status.trajs.Add(RootSeries.GetDirection(sample.Index).GetRelativeDirectionTo(root));
            status.styles.Add(StyleSeries.Values[sample.Index]);
        }


        //Input Goals
        for (int i = 0; i < TimeSeries.KeyCount; i++)
        {
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            status.goals.Add(GoalSeries.Transformations[sample.Index].GetPosition().GetRelativePositionTo(root));
            status.goals.Add(GoalSeries.Transformations[sample.Index].GetForward().GetRelativeDirectionTo(root));
            status.styGoals.Add(GoalSeries.Values[sample.Index]);
        }

        //Input Environment
        Environment.Sense(root, LayerMask.GetMask("Default", "Interaction"));
        status.env = Environment.Occupancies;

        //Input Geometry
        for (int i = 0; i < Geometry.Points.Length; i++)
        {
            status.geoms.Add(Geometry.References[i].GetRelativePositionTo(root));
            status.geomsOcc.Add(Geometry.Occupancies[i]);
        }

        //Setup Gating Features
        // NeuralNetwork.Feed(GenerateGating());
        status.gats = GenerateGating();

        return status;
    }

    protected override void Read()
    {
        foreach (var item in NameToObject)
            item.Value.SetActive(false);
        // Get All transforms of bones
        animBones = animDriver.getAllBonesTransform();

        //Update Past State
        for (int i = 0; i < TimeSeries.Pivot; i++)
        {
            TimeSeries.Sample sample = TimeSeries.Samples[i];
            PhaseSeries.Values[i] = PhaseSeries.Values[i + 1];
            RootSeries.SetPosition(i, RootSeries.GetPosition(i + 1));
            RootSeries.SetDirection(i, RootSeries.GetDirection(i + 1));
            for (int j = 0; j < StyleSeries.Styles.Length; j++)
            {
                StyleSeries.Values[i][j] = StyleSeries.Values[i + 1][j];
            }

            for (int j = 0; j < ContactSeries.Bones.Length; j++)
            {
                ContactSeries.Values[i][j] = ContactSeries.Values[i + 1][j];
            }

            GoalSeries.Transformations[i] = GoalSeries.Transformations[i + 1];
            for (int j = 0; j < GoalSeries.Actions.Length; j++)
            {
                GoalSeries.Values[i][j] = GoalSeries.Values[i + 1][j];
            }
        }

        //Get Root
        Matrix4x4 root = RootSeries.Transformations[TimeSeries.Pivot];

        //Read Posture
        Vector3[] positions = new Vector3[Actor.Bones.Length];
        Vector3[] forwards = new Vector3[Actor.Bones.Length];
        Vector3[] upwards = new Vector3[Actor.Bones.Length];
        Vector3[] velocities = new Vector3[Actor.Bones.Length];
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            Vector3 position = NeuralNetwork.ReadVector3().GetRelativePositionFrom(root);
            Vector3 forward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
            Vector3 upward = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
            Vector3 velocity = NeuralNetwork.ReadVector3().GetRelativeDirectionFrom(root);
            positions[i] = Vector3.Lerp(Actor.Bones[i].Transform.position + velocity / GetFramerate(), position, 0.5f);
            forwards[i] = forward;
            upwards[i] = upward;
            velocities[i] = velocity;
        }

        //Read Inverse Pose
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            PosePrediction[i] = NeuralNetwork.ReadVector3().GetRelativePositionFrom(RootSeries.Transformations.Last());
            velocities[i] = Vector3.Lerp(velocities[i],
                GetFramerate() * (PosePrediction[i] - Actor.Bones[i].Transform.position), 1f / GetFramerate());
        }

        //Read Future Trajectory
        for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
        {
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            Vector3 pos = NeuralNetwork.ReadXZ().GetRelativePositionFrom(root);
            Vector3 dir = NeuralNetwork.ReadXZ().normalized.GetRelativeDirectionFrom(root);
            RootSeries.SetPosition(sample.Index, pos);
            RootSeries.SetDirection(sample.Index, dir);
            float[] styles = NeuralNetwork.Read(StyleSeries.Styles.Length);
            for (int j = 0; j < styles.Length; j++)
            {
                styles[j] = Mathf.Clamp(styles[j], 0f, 1f);
            }

            StyleSeries.Values[sample.Index] = styles;

            RootPrediction[i - 6] = Matrix4x4.TRS(pos, Quaternion.LookRotation(dir, Vector3.up), Vector3.one);
        }

        //Read Inverse Trajectory
        for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
        {
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            Matrix4x4 goal = GoalSeries.Transformations[TimeSeries.Pivot];
            goal[1, 3] = 0f;
            Vector3 pos = NeuralNetwork.ReadXZ().GetRelativePositionFrom(goal);
            Vector3 dir = NeuralNetwork.ReadXZ().normalized.GetRelativeDirectionFrom(goal);
            if (i > TimeSeries.PivotKey)
            {
                Matrix4x4 pivot = RootSeries.Transformations[sample.Index];
                pivot[1, 3] = 0f;
                Matrix4x4 reference = GoalSeries.Transformations[sample.Index];
                reference[1, 3] = 0f;
                float distance = Vector3.Distance(pivot.GetPosition(), reference.GetPosition());
                float weight = Mathf.Pow((float) (i - 6) / 7f, distance * distance);

                RootSeries.SetPosition(sample.Index, Vector3.Lerp(RootSeries.GetPosition(sample.Index), pos, weight));
                RootSeries.SetDirection(sample.Index,
                    Vector3.Slerp(RootSeries.GetDirection(sample.Index), dir, weight));
            }

            GoalPrediction[i - 6] = Matrix4x4.TRS(pos, Quaternion.LookRotation(dir, Vector3.up), Vector3.one);
        }

        //Read and Correct Goals
        for (int i = 0; i < TimeSeries.KeyCount; i++)
        {
            float weight = TimeSeries.GetWeight1byN1(TimeSeries.GetKey(i).Index, 2f);
            TimeSeries.Sample sample = TimeSeries.GetKey(i);
            Vector3 pos = NeuralNetwork.ReadVector3().GetRelativePositionFrom(root);
            Vector3 dir = NeuralNetwork.ReadVector3().normalized.GetRelativeDirectionFrom(root);
            float[] actions = NeuralNetwork.Read(GoalSeries.Actions.Length);
            for (int j = 0; j < actions.Length; j++)
            {
                actions[j] = Mathf.Clamp(actions[j], 0f, 1f);
            }

            GoalSeries.Transformations[sample.Index] = Utility.Interpolate(GoalSeries.Transformations[sample.Index],
                Matrix4x4.TRS(pos, Quaternion.LookRotation(dir, Vector3.up), Vector3.one), weight * NetworkControl);
            GoalSeries.Values[sample.Index] =
                Utility.Interpolate(GoalSeries.Values[sample.Index], actions, weight * NetworkControl);
        }

        //Read Future Contacts
        float[] contacts = NeuralNetwork.Read(ContactSeries.Bones.Length);
        for (int i = 0; i < contacts.Length; i++)
        {
            contacts[i] = Mathf.Clamp(contacts[i], 0f, 1f);
        }

        ContactSeries.Values[TimeSeries.Pivot] = contacts;

        //Read Phase Update
        float phase = PhaseSeries.Values[TimeSeries.Pivot];
        for (int i = TimeSeries.PivotKey; i < TimeSeries.KeyCount; i++)
        {
            PhaseSeries.Values[TimeSeries.GetKey(i).Index] = Mathf.Repeat(phase + NeuralNetwork.Read(), 1f);
        }

        //Interpolate Current to Future Trajectory
        for (int i = 0; i < TimeSeries.Samples.Length; i++)
        {
            float weight = (float) (i % TimeSeries.Resolution) / TimeSeries.Resolution;
            TimeSeries.Sample sample = TimeSeries.Samples[i];
            TimeSeries.Sample prevSample = TimeSeries.GetPreviousKey(i);
            TimeSeries.Sample nextSample = TimeSeries.GetNextKey(i);
            //PhaseSeries.Values[sample.Index] = Mathf.Lerp(PhaseSeries.Values[prevSample.Index], PhaseSeries.Values[nextSample.Index], weight);
            RootSeries.SetPosition(sample.Index,
                Vector3.Lerp(RootSeries.GetPosition(prevSample.Index), RootSeries.GetPosition(nextSample.Index),
                    weight));
            RootSeries.SetDirection(sample.Index,
                Vector3.Slerp(RootSeries.GetDirection(prevSample.Index), RootSeries.GetDirection(nextSample.Index),
                    weight));
            GoalSeries.Transformations[sample.Index] = Utility.Interpolate(GoalSeries.Transformations[prevSample.Index],
                GoalSeries.Transformations[nextSample.Index], weight);
            for (int j = 0; j < StyleSeries.Styles.Length; j++)
            {
                StyleSeries.Values[i][j] = Mathf.Lerp(StyleSeries.Values[prevSample.Index][j],
                    StyleSeries.Values[nextSample.Index][j], weight);
            }

            for (int j = 0; j < GoalSeries.Actions.Length; j++)
            {
                GoalSeries.Values[i][j] = Mathf.Lerp(GoalSeries.Values[prevSample.Index][j],
                    GoalSeries.Values[nextSample.Index][j], weight);
            }
        }

        //Assign Posture
        transform.position = RootSeries.GetPosition(TimeSeries.Pivot);
        transform.rotation = RootSeries.GetRotation(TimeSeries.Pivot);
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            Actor.Bones[i].Velocity = velocities[i];
            Actor.Bones[i].Transform.position = positions[i];
            Actor.Bones[i].Transform.rotation = Quaternion.LookRotation(forwards[i], upwards[i]);
            Actor.Bones[i].ApplyLength();
        }

        // **************************************************************************

        /*
         * get current based action
         * 考虑一种情况：执行坐到椅子上（过程：站立-走过去-做下去，不能是单独的坐代替）
         * recommend to use the following calculation
         * only consider last frame
         * 0-stand, 1-walk, 2-run, 3-carry, 4-open, 5-sit
         */
        int mLab = 0;
        //int sel_frame = 0;
        //int sel_frame = TimeSeries.Samples.Length - 1;
        int sel_frame = TimeSeries.Samples.Length / 2; // 61/2=30
        //Debug.Log("==============>"+TimeSeries.Samples.Length);
        for (int i = 1; i < StyleSeries.Values[sel_frame].Length; i++)
        {
            // 7=[1,0,0,0,0,0,0]
            if (StyleSeries.Values[sel_frame][i] > StyleSeries.Values[sel_frame][mLab])
            {
                mLab = i;
            }
        }
        // Debug.Log(mLab);
        // Debug.Log(action.actASC);
        // Debug.Log(action.style);
        
        Dictionary<string, Quaternion> nsm_rots = new Dictionary<string, Quaternion>();
        for (int i = 0; i < Actor.Bones.Length; i++)
        {
            Actor.Bone bone = Actor.Bones[i];
            nsm_rots.Add(bone.GetName(), bone.Transform.rotation);
        }


        if (dealText.action.bothMotion)
        {
            // assign upper half bones of motion, while lower half NSM
            // compute rotation of all bones of nsm(origin frame must be Tpose)
            // find relevant motion bvh, and compute rotations
            // drive the new human model in the scene (not matter the nsm model)

            
            //find relevant motion BVH，and compute rotations, key: search motion
            string upperMotion = BVHSearcher.searchMotionForKey(dealText.action.style);

            // 处理sit坐下的过程中，上半身动作开始的时间
            if (upperMotion != "" && mLab != dealText.action.actASC)
            {
                upperMotion = "";
                dealText.action.IsSitBegin = false;
            }
            else if ((byte) ActionType.Sit == mLab)
            {
                if (!dealText.action.IsSitBegin)
                {
                    dealText.action.IsSitBegin = true;
                    dealText.action.timeSitStart = Time.realtimeSinceStartup;
                }
                else
                {
                    if (dealText.action.timeNow - dealText.action.timeSitStart >= 1f)
                        AddPosRec();
                }
            }
            else if ((byte) ActionType.Idle == mLab)
            {
                // 优化 stand 的时候开始记录
                AddPosRec();
            }

            if (mLab == 0 && upperMotion == "")
            {
                upperMotion = "idle";
            }
            
            // if (mLab == 1 && upperMotion == "")
            // {
            //     upperMotion = "walk";
            // }
            
            animDriver.searchMotion(upperMotion); //file pth: search result
            animDriver.updateRotations(nsm_rots, Actor.Bones[0].Transform.position, mLab);

            // 辅助动作交互物体跟随
            if (dealText.action.style != "null" && dealText.action.hasStyleObj)
            {
                if (dealText.action.styleObjs[0].modelName.Equals("drumsticks"))
                {
                    foreach (var item in NameToObject)
                        if (item.Key.Equals("drumstick1") || item.Key.Equals("drumstick2"))
                            item.Value.SetActive(true);
                }
                else
                {
                    foreach (var item in NameToObject)
                        if (item.Key.Equals(dealText.action.styleObjs[0].modelName))
                            item.Value.SetActive(true);
                }
            }
        }
        else
        {
            // 处理没有辅助动作时，角色的骨骼位置
            string upperMotion = "";
            if (mLab == 0)
            {
                upperMotion = "idle";
                animDriver.searchMotion(upperMotion); //file pth: search result
            }
            animDriver.updateRotations(nsm_rots, Actor.Bones[0].Transform.position, mLab);
            
            // if (mLab == 1)
            // {
            //     string upperMotion = "walk";
            //     animDriver.searchMotion(upperMotion); //file pth: search result
            //     animDriver.updateRotations(nsm_rots, Actor.Bones[0].Transform.position);
            // }
            
        }

        // **************************************************************************
    }


    private string[] styleObjects = {"phone", "cap", "microphone", "sandwich", "bottle", "drumstick1", "drumstick2"};
    private Dictionary<string, GameObject> NameToObject = new Dictionary<string, GameObject>();

    
    [Header("Critical Value")]
    public float walk = 2.0f;
    public float turn = 80f;
    // float turnAngles = 90f;

    private void Default()
    {
        if (dealText.action.hasTarget && (dealText.action.actASC == (byte) ActionType.Walk ||
                                          dealText.action.actASC == (byte) ActionType.Run))
        {
            // assign target
            List<TargetInfo> targets = dealText.action.targets;
            int targetNum = targets.Count;
            for (int i = 0; i < targetNum; i++)
            {
                Interaction target = ModelsManager.GetClosestModel(transform, targets[i].modelName);
                if (target == null)
                {
                    Debug.Log("The model you input does not exist.");
                    if (i == targetNum - 1)
                        dealText.action.isCompleted = true;
                    continue;
                }

                ApplyDynamicGoal(
                    RootSeries.Transformations[TimeSeries.Pivot],
                    (target.transform.position - transform.position).GetRelativeDirectionTo(transform.GetWorldMatrix())
                    .normalized,
                    Vector3.SignedAngle(transform.forward, target.transform.position - transform.position, Vector3.up) *
                    2f,
                    Signals
                );
                //ApplyStaticGoal(
                //    target.GetCenter().GetPosition(),
                //    Vector3.ProjectOnPlane(target.GetCenter().GetPosition() - transform.position, Vector3.up).normalized,
                //    Signals
                //)
                if (Vector3.Distance(target.transform.position, transform.position) < walk)
                {
                    dealText.action.isCompleted = true;
                }
            }
        }
        else if (dealText.action.tarOptim != null)
        {
            ApplyDynamicGoal(
                RootSeries.Transformations[TimeSeries.Pivot],
                (dealText.action.tarOptim[0].GetPosition() - transform.position)
                .GetRelativeDirectionTo(transform.GetWorldMatrix()).normalized,
                // Vector3.SignedAngle(transform.forward, dealText.action.tarOptim[0].GetPosition() - transform.position, Vector3.up) * 2f,
                Vector3.SignedAngle(dealText.action.tarOptim[0].GetForward(), dealText.action.tarOptim[0].GetForward(),
                    Vector3.up) * 2f,
                Signals
            );
            // Debug.Log(dealText.action.tarOptim[0].GetForward());
            // if (Vector3.Distance(dealText.action.tarOptim[0].GetPosition(), transform.position) < 1.5f)
            //     dealText.action.isCompleted = true;
        }
        else
        {
            // ****************************************************************
            // 计算 look at 的角度，并赋予向左向右的 style
            if (dealText.action.actASC == (byte) ActionType.Turn && dealText.action.hasTarget &&
                !dealText.action.angleComputed)
            {
                Transform styleObj = GameObject.Find(dealText.action.targets[0].modelName).transform;

                Vector3 fromVector = transform.forward;
                Vector3 toVector = (styleObj.position - transform.position).ZeroY();

                float angle = Vector3.Angle(fromVector, toVector); //求出两向量之间的夹角
                Vector3 normal = Vector3.Cross(fromVector, toVector); //叉乘求出法线向量

                float sign = Mathf.Sign(Vector3.Dot(normal, Vector3.up));

                if (sign >= 0)
                    dealText.action.style = "right";
                else if (sign < 0)
                    dealText.action.style = "left";

                turn = Math.Abs(angle);
                dealText.action.angleComputed = true;
            }
            // *******************************************************************

            // Stands timing
            if (!dealText.action.IsSitBegin)
            {
                dealText.action.IsSitBegin = true;
                dealText.action.timeSitStart = Time.realtimeSinceStartup;
                dealText.action.timeNow = Time.realtimeSinceStartup;
            }

            // assign position
            ApplyDynamicGoal(
                RootSeries.Transformations[TimeSeries.Pivot],
                Controller.QueryMove(dealText.action, Signals),
                Controller.QueryTurn(dealText.action, 90f),
                Signals
            );
            if (dealText.action.actASC == (byte) ActionType.Turn &&
                Math.Abs(dealText.action.angleTurnN.y - dealText.action.angleTurnP.y) > turn)
            {
                dealText.action.isCompleted = true;
            }

            if (dealText.action.IsSitBegin && dealText.action.actASC == (byte) ActionType.Idle &&
                dealText.action.timeNow - dealText.action.timeSitStart >= stand_Time)
            {
                dealText.action.isCompleted = true;
            }
        }

        Geometry.Setup(Geometry.Resolution);
        Geometry.Sense(RootSeries.Transformations[TimeSeries.Pivot], LayerMask.GetMask("Interaction"), Vector3.zero,
            InteractionSmoothing);
    }
    
    [Header("Time Controller")]
    public float stand_Time = 20f;

    public float sit_Time = 5f;

    // ***************************************

    private void showView(Dictionary<string, Vector3> dic)
    {
        foreach (string s in dic.Keys)
        {
            GameObject tr = GameObject.CreatePrimitive(PrimitiveType.Sphere);
            tr.transform.position = dic[s];
            tr.transform.localScale = new Vector3(0.03f, 0.03f, 0.03f);

            Material m_tr = tr.GetComponent<Renderer>().material;
            m_tr.color = Color.blue;

            GameObject pa = GameObject.Find("DispPoints");
            tr.transform.parent = pa.transform;
        }
    }

    public void AddPosRec()
    {
        Dictionary<string, Vector3> pos = new Dictionary<string, Vector3>();

        pos.Add("LeftMiddleDistal", animBones["LeftMiddleDistal"].position);
        pos.Add("RightMiddleDistal", animBones["RightMiddleDistal"].position);
        pos.Add("LeftHand", animBones["LeftHand"].position);
        pos.Add("RightHand", animBones["RightHand"].position);

        ProcessText.PosRecord.Add(pos);
        // showView(pos);
    }

    // ***************************************

    private IEnumerator Sit()
    {
        // float sit_Time = 10f;

        Controller.Signal signal = Controller.GetSignal("Sit");

        string targetName = "chair"; //default target of sit

        if (dealText.action.hasTarget)
            targetName = dealText.action.targets[0].modelName;
        Interaction targetModel = ModelsManager.GetClosestModel(transform, targetName);

        float threshold = 0.1f;

        if (targetModel != null)
        {
            Controller.ActiveInteraction = targetModel;
            IsInteracting = true;
            while (dealText.action.actASC == (byte) ActionType.Sit)
            {
                // sit for five seconds if next action exists, or sit always
                if (!dealText.action.IsSitBegin &&
                    Vector3.Distance(targetModel.GetContact("Hips").GetPosition(), transform.position) < 0.8f)
                {
                    dealText.action.IsSitBegin = true;
                    dealText.action.timeSitStart = Time.realtimeSinceStartup;
                    dealText.action.timeNow = Time.realtimeSinceStartup;
                }

                // 人物已经坐下来
                if (dealText.action.IsSitBegin)
                {
                    // 上层改变action之后,这里要将之前的 Coroutine 跳出来
                    if (dealText.IsStayingActionBreak()) break;

                    if (dealText.IsOptim())
                    {
                        // 在优化中，不管接下来有无动作，等待5秒后结束
                        if (dealText.action.IsSitBegin &&
                            (dealText.action.timeNow - dealText.action.timeSitStart > sit_Time))
                        {
                            dealText.action.isCompleted = true;
                            // break;
                        }
                    }
                    else
                    {
                        // next action: null -> continue 
                        // next action: sit -> 1. targets changed: wait 6s, break; 2. not changed: continue
                        // next action: other -> wait 6s, break
                        if (dealText.IdxAct + 1 < dealText.actionNum)
                        {
                            // next not sit
                            if (dealText.info.actions[dealText.IdxAct + 1].actASC != (byte) ActionType.Sit)
                            {
                                // next action: other -> wait 6s, next
                                if (dealText.action.IsSitBegin &&
                                    (dealText.action.timeNow - dealText.action.timeSitStart > sit_Time))
                                {
                                    dealText.action.isCompleted = true;
                                    //break;
                                }
                            }
                            else // next is sit
                            {
                                if (dealText.info.targetsNextChangedExceptNone(dealText.IdxAct, dealText.IdxAct + 1))
                                {
                                    // next target model changed
                                    // next action: other -> wait 6s, next
                                    if (dealText.action.IsSitBegin &&
                                        (dealText.action.timeNow - dealText.action.timeSitStart > sit_Time))
                                    {
                                        dealText.action.isCompleted = true;
                                        // break;
                                    }
                                }
                                else
                                {
                                    Debug.Log("timesit" + dealText.action.timeSitStart);
                                    Debug.Log("timenow" + dealText.action.timeNow);

                                    if (dealText.action.style == "null" &&
                                        dealText.info.actions[dealText.IdxAct + 1].style != "null")
                                    {
                                        dealText.IdxAct += 1;
                                        dealText.action = dealText.info.actions[dealText.IdxAct];
                                        Debug.Log("change" + dealText.action.timeNow);
                                    }
                                    else
                                    {
                                        if (dealText.action.timeNow - dealText.action.timeSitStart > sit_Time)
                                        {
                                            dealText.IdxAct += 1;
                                            dealText.action = dealText.info.actions[dealText.IdxAct];
                                        }
                                    }
                                }
                                // next target model do not changed
                                // continue
                            }
                        }
                    } // end isOptim
                }

                // 文本输入打断现有动作
                if (dealText.IsChangedOptim) break;

                // sit down
                ApplyStaticGoal(targetModel.GetContact("Hips").GetPosition(),
                    targetModel.GetContact("Hips").GetForward(), Signals);
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(targetModel.GetCenter(), LayerMask.GetMask("Interaction"), targetModel.GetExtents(),
                    InteractionSmoothing);
                yield return new WaitForSeconds(0f);
            }

            // stand up
            while (StyleSeries.GetStyle(TimeSeries.Pivot, "Sit") > threshold)
            {
                ApplyDynamicGoal(
                    RootSeries.Transformations[TimeSeries.Pivot],
                    Controller.QueryMove(new ActionInfo((byte) ActionType.Walk), SignalWalk),
                    Controller.QueryTurn(new ActionInfo((byte) ActionType.Walk), 90f),
                    SignalWalk
                );
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(targetModel.GetCenter(), LayerMask.GetMask("Interaction"), targetModel.GetExtents(),
                    InteractionSmoothing);
                yield return new WaitForSeconds(0f);
            }

            IsInteracting = false;
            Controller.ActiveInteraction = null;
        }
        else
            Debug.Log("The model you input does not exist.");

        // dealText.action.isCompleted = true;
        dealText.setStayingActionBreak(false);
    }

    private IEnumerator Open()
    {
        Debug.Log("yes");
        Controller.Signal signal = Controller.GetSignal("Open");
        Interaction targetModel = ModelsManager.GetClosestModel(transform, "door");

        if (targetModel != null)
        {
            Controller.ActiveInteraction = targetModel;
            IsInteracting = true;
            while (dealText.action.actASC == (byte) ActionType.Open)
            {
                float angle = Vector3.Angle(targetModel.GetCenter().GetForward(),
                    transform.position - targetModel.transform.position);
                float distance = Vector3.Distance(transform.position, targetModel.transform.position);
                if (angle < 90 && distance > 0.2f)
                {
                    break;
                }

                ApplyStaticGoal(targetModel.GetCenter().GetPosition(), targetModel.GetCenter().GetForward(), Signals);
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(targetModel.GetCenter(), LayerMask.GetMask("Interaction"), targetModel.GetExtents(),
                    InteractionSmoothing);
                yield return new WaitForSeconds(0f);
            }

            IsInteracting = false;
            Controller.ActiveInteraction = null;
        }
        else
            Debug.Log("The model you input does not exist.");

        dealText.action.isCompleted = true;
    }

    private IEnumerator Carry()
    {
        Controller.Signal signal = Controller.GetSignal("Carry");

        // object to carry, empty if none, then exit
        Interaction objectCarry = null;
        if (dealText.action.objectName == null)
            Debug.Log("You did not write an object to carry. Please enter again.");
        else
            objectCarry = ModelsManager.GetClosestModel(transform, dealText.action.objectName);


        // target to carry to, output error if none
        Interaction target = null;
        if (objectCarry != null)
        {
            if (dealText.action.hasTarget)
            {
                string targetName = dealText.action.targets[0].modelName;
                target = ModelsManager.GetClosestModel(objectCarry.transform, targetName);
            }
            else
            {
                Debug.Log("Please specify the destination for the carrying action.");
            }
        }


        if (objectCarry != null && target != null)
        {
            Controller.ActiveInteraction = objectCarry;
            // Debug.Log("Carrying started...");
            IsInteracting = true;

            float duration = 0.5f;
            float threshold = 0.2f;

            Vector3 deltaPos = new Vector3(0f, -0.15f, 0.2f);
            Quaternion deltaRot = Quaternion.Euler(-30f, 0f, 0f);

            float height = 1f;

            Matrix4x4 GetObjectMatrix(float staticness)
            {
                Matrix4x4 right = Actor.GetBoneTransformation("RightWrist");
                Matrix4x4 left = Actor.GetBoneTransformation("LeftWrist");

                Quaternion rotation = Quaternion.Slerp(
                    Quaternion.LookRotation(left.GetRight(), left.GetForward()),
                    Quaternion.LookRotation(-right.GetRight(), right.GetForward()),
                    0.5f
                );
                rotation *= deltaRot;
                rotation = Quaternion.Slerp(
                    rotation,
                    transform.rotation,
                    Utility.Normalise(staticness, 0f, 1f, 0.5f, 1f)
                );

                return Matrix4x4.TRS(
                    Vector3.Lerp(left.GetPosition(), right.GetPosition(), 0.5f) + rotation * deltaPos,
                    rotation,
                    objectCarry.transform.lossyScale
                );
            }

            bool HasContact()
            {
                float left = ContactSeries.GetContact(TimeSeries.Pivot, "LeftWrist");
                float right = ContactSeries.GetContact(TimeSeries.Pivot, "RightWrist");
                return right > 0.5f && left > 0.5f || (left + right) > 1f;
            }

            //Move to the target location
            // Debug.Log("Approaching to lift object...");
            while (dealText.action.actASC == (byte) ActionType.Carry)
            {
                dealText.action.CarryToPlusIdle = true;
                ApplyStaticGoal(
                    objectCarry.GetCenter().GetPosition(),
                    objectCarry.GetCenter().GetForward(),
                    Signals
                );
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(objectCarry.GetCenter(), LayerMask.GetMask("Interaction"), objectCarry.GetExtents(),
                    InteractionSmoothing);
                Geometry.Retransform(objectCarry.GetOrigin(GoalSeries.Transformations[TimeSeries.Pivot]));
                if (Vector3.Distance(GetObjectMatrix(0f).GetPosition(), objectCarry.transform.position) < threshold)
                    break;

                yield return new WaitForSeconds(0f);
            }

            //Move the object from the surface to the hands
            // Debug.Log("Picking object...");
            float tPick = Time.time;
            Vector3 pos = objectCarry.transform.position;
            Quaternion rot = objectCarry.transform.rotation;
            while (dealText.action.actASC == (byte) ActionType.Carry && HasContact())
            {
                float ratio = Mathf.Clamp((Time.time - tPick) / duration, 0f, 1f);
                Matrix4x4 m = GetObjectMatrix(1f - ratio);
                objectCarry.transform.position = Vector3.Lerp(pos, m.GetPosition(), ratio);
                objectCarry.transform.rotation = Quaternion.Slerp(rot, m.GetRotation(), ratio);
                ApplyStaticGoal(objectCarry.GetCenter().GetPosition(), objectCarry.GetCenter().GetForward(), Signals);
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(objectCarry.GetCenter(), LayerMask.GetMask("Interaction"), objectCarry.GetExtents(),
                    InteractionSmoothing);
                Geometry.Retransform(objectCarry.GetOrigin(GoalSeries.Transformations[TimeSeries.Pivot]));
                if (ratio >= 1f)
                {
                    break;
                }

                yield return new WaitForSeconds(0f);
            }

            //Move around with the object
            //Debug.Log("Carrying object and moving...");
            while (HasContact())
            {
                Matrix4x4 m = GetObjectMatrix(0f);
                objectCarry.transform.position = m.GetPosition();
                objectCarry.transform.rotation = m.GetRotation();

                ApplyDynamicGoal(
                    objectCarry.GetCenter(),
                    (target.transform.position - transform.position).GetRelativeDirectionTo(transform.GetWorldMatrix())
                    .normalized + new Vector3(0f, height - objectCarry.transform.position.y, 0f),
                    Vector3.SignedAngle(transform.forward, target.transform.position - transform.position, Vector3.up) *
                    2f,
                    Signals
                );

                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(objectCarry.GetCenter(), LayerMask.GetMask("Interaction"), objectCarry.GetExtents(),
                    InteractionSmoothing);
                Geometry.Retransform(objectCarry.GetOrigin(GoalSeries.Transformations[TimeSeries.Pivot]));

                if (Vector3.Distance(transform.position, target.transform.position) < 2f)
                {
                    break;
                }

                dealText.action.CarryToPlusIdle = false;
                dealText.action.CarryToPlusWalk = true;

                yield return new WaitForSeconds(0f);
            }

            //Perform motions to start placing the object
            Debug.Log("Transitioning to placing down object...");
            while (HasContact())
            {
                Matrix4x4 m = GetObjectMatrix(0f);
                objectCarry.transform.position = m.GetPosition();
                objectCarry.transform.rotation = m.GetRotation();
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(objectCarry.GetCenter(), LayerMask.GetMask("Interaction"), objectCarry.GetExtents(),
                    InteractionSmoothing);
                Geometry.Retransform(objectCarry.GetOrigin(GoalSeries.Transformations[TimeSeries.Pivot]));

                Vector3 surface = Utility.ProjectGround(objectCarry.transform.position,
                    LayerMask.GetMask("Default", "Ground"));
                ApplyStaticGoal(
                    surface,
                    Vector3.ProjectOnPlane(m.GetForward(), Vector3.up).normalized,
                    Signals
                );

                if (Vector3.Distance(surface, objectCarry.transform.position) < 0.5f)
                {
                    break;
                }

                dealText.action.CarryToPlusWalk = false;
                dealText.action.CarryToIdle = true;

                yield return new WaitForSeconds(0f);
            }

            //Make sure the object is again placed on the surface
            Debug.Log("Placing object...");
            float tPlace = Time.time;
            Vector3 aPosPlace = objectCarry.transform.position;
            Vector3 bPosPlace = Utility.ProjectGround(aPosPlace, LayerMask.GetMask("Default", "Ground"));
            Quaternion aRotPlace = objectCarry.transform.rotation;
            Quaternion bRotPlace =
                Quaternion.LookRotation(Vector3.ProjectOnPlane(aRotPlace.GetForward(), Vector3.up), Vector3.up);
            while (true)
            {
                float ratio = Mathf.Clamp((Time.time - tPlace) / duration, 0f, 1f);
                objectCarry.transform.position = Vector3.Lerp(aPosPlace, bPosPlace, ratio);
                objectCarry.transform.rotation = Quaternion.Slerp(aRotPlace, bRotPlace, ratio);
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(objectCarry.GetCenter(), LayerMask.GetMask("Interaction"), objectCarry.GetExtents(),
                    InteractionSmoothing);
                Geometry.Retransform(objectCarry.GetOrigin(GoalSeries.Transformations[TimeSeries.Pivot]));
                ApplyStaticGoal(transform.position, transform.forward, SignalIdle);
                if (ratio >= 1f)
                {
                    break;
                }

                yield return new WaitForSeconds(0f);
            }

            //Debug.Log("End");
            while (StyleSeries.GetStyle(TimeSeries.Pivot, "Carry") > 0.1)
            {
                ApplyDynamicGoal(
                    RootSeries.Transformations[TimeSeries.Pivot],
                    Controller.QueryMove(new ActionInfo((byte) ActionType.Walk), Signals),
                    Controller.QueryTurn(new ActionInfo((byte) ActionType.Walk), 90f),
                    Signals
                );
                Geometry.Setup(Geometry.Resolution);
                Geometry.Sense(objectCarry.GetCenter(), LayerMask.GetMask("Interaction"), objectCarry.GetExtents(),
                    InteractionSmoothing);

                yield return new WaitForSeconds(0f);
            }

            IsInteracting = false;
            // Debug.Log("Carrying finished...");
            Controller.ActiveInteraction = null;
        }
        else if (objectCarry == null && dealText.action.objectName != null)
        {
            Debug.Log("The model you input does not exist.");
        }

        dealText.action.isCompleted = true;
    }

    protected override void Postprocess()
    {
        Matrix4x4 rightFoot = Actor.GetBoneTransformation(ContactSeries.Bones[3]);
        Matrix4x4 leftFoot = Actor.GetBoneTransformation(ContactSeries.Bones[4]);
        RightFootIK.Objectives[0].SetTarget(rightFoot.GetPosition(), 1f - ContactSeries.Values[TimeSeries.Pivot][3]);
        RightFootIK.Objectives[0].SetTarget(rightFoot.GetRotation());
        LeftFootIK.Objectives[0].SetTarget(leftFoot.GetPosition(), 1f - ContactSeries.Values[TimeSeries.Pivot][4]);
        LeftFootIK.Objectives[0].SetTarget(leftFoot.GetRotation());
        RightFootIK.Solve();
        LeftFootIK.Solve();

        Transform rightToe = Actor.FindBone("RightToe").Transform;
        Vector3 rightPos = rightToe.transform.position;
        rightPos.y = Mathf.Max(rightPos.y, 0.02f);
        rightToe.position = rightPos;

        Transform leftToe = Actor.FindBone("LeftToe").Transform;
        Vector3 leftPos = leftToe.transform.position;
        leftPos.y = Mathf.Max(leftPos.y, 0.02f);
        leftToe.position = leftPos;
    }

    protected override void OnGUIDerived()
    {
        if (!ShowGUI)
        {
            return;
        }

        if (ShowGoal)
        {
            GoalSeries.GUI();
        }

        if (ShowCurrent)
        {
            StyleSeries.GUI();
        }

        if (ShowPhase)
        {
            PhaseSeries.GUI();
        }

        if (ShowContacts)
        {
            ContactSeries.GUI();
        }
    }

    protected override void OnRenderObjectDerived()
    {
        Controller.Draw();

        if (ShowRoot)
        {
            RootSeries.Draw();
        }

        if (ShowGoal)
        {
            GoalSeries.Draw();
        }

        if (ShowCurrent)
        {
            StyleSeries.Draw();
        }

        if (ShowPhase)
        {
            PhaseSeries.Draw();
        }

        if (ShowContacts)
        {
            ContactSeries.Draw();
        }

        if (ShowEnvironment)
        {
            Environment.Draw(UltiDraw.Mustard.Transparent(0.25f));
        }

        if (ShowInteraction)
        {
            Geometry.Draw(UltiDraw.Cyan.Transparent(0.25f));
        }

        if (ShowBiDirectional)
        {
            UltiDraw.Begin();
            for (int i = 0; i < PosePrediction.Length; i++)
            {
                UltiDraw.DrawSphere(PosePrediction[i], Quaternion.identity, 0.05f, UltiDraw.Magenta);
            }

            for (int i = 0; i < RootPrediction.Length; i++)
            {
                UltiDraw.DrawCircle(RootPrediction[i].GetPosition(), 0.05f, UltiDraw.DarkRed.Darken(0.5f));
                UltiDraw.DrawArrow(RootPrediction[i].GetPosition(),
                    RootPrediction[i].GetPosition() + 0.1f * RootPrediction[i].GetForward(), 0f, 0f, 0.025f,
                    UltiDraw.DarkRed);
                if (i < RootPrediction.Length - 1)
                {
                    UltiDraw.DrawLine(RootPrediction[i].GetPosition(), RootPrediction[i + 1].GetPosition(),
                        UltiDraw.Black);
                }
            }

            for (int i = 0; i < GoalPrediction.Length; i++)
            {
                UltiDraw.DrawCircle(GoalPrediction[i].GetPosition(), 0.05f, UltiDraw.DarkGreen.Darken(0.5f));
                UltiDraw.DrawArrow(GoalPrediction[i].GetPosition(),
                    GoalPrediction[i].GetPosition() + 0.1f * GoalPrediction[i].GetForward(), 0f, 0f, 0.025f,
                    UltiDraw.DarkGreen);
                if (i < GoalPrediction.Length - 1)
                {
                    UltiDraw.DrawLine(GoalPrediction[i].GetPosition(), GoalPrediction[i + 1].GetPosition(),
                        UltiDraw.Black);
                }
            }

            UltiDraw.End();
        }
    }

    private float[] GenerateGating()
    {
        List<float> values = new List<float>();

        for (int k = 0; k < TimeSeries.KeyCount; k++)
        {
            int index = TimeSeries.GetKey(k).Index;
            Vector2 phase = Utility.PhaseVector(PhaseSeries.Values[index]);
            for (int i = 0; i < StyleSeries.Styles.Length; i++)
            {
                float magnitude = StyleSeries.Values[index][i];
                magnitude = Utility.Normalise(magnitude, 0f, 1f, -1f, 1f);
                values.Add(magnitude * phase.x);
                values.Add(magnitude * phase.y);
            }

            for (int i = 0; i < GoalSeries.Actions.Length; i++)
            {
                float magnitude = GoalSeries.Values[index][i];
                magnitude = Utility.Normalise(magnitude, 0f, 1f, -1f, 1f);
                Matrix4x4 root = RootSeries.Transformations[index];
                root[1, 3] = 0f;
                Matrix4x4 goal = GoalSeries.Transformations[index];
                goal[1, 3] = 0f;
                float distance = Vector3.Distance(root.GetPosition(), goal.GetPosition());
                float angle = Quaternion.Angle(root.GetRotation(), goal.GetRotation());
                values.Add(magnitude * phase.x);
                values.Add(magnitude * phase.y);
                values.Add(magnitude * distance * phase.x);
                values.Add(magnitude * distance * phase.y);
                values.Add(magnitude * angle * phase.x);
                values.Add(magnitude * angle * phase.y);
            }
        }

        return values.ToArray();
    }

    private void ApplyStaticGoal(Vector3 position, Vector3 direction, float[] actions)
    {
        //Transformations
        for (int i = 0; i < TimeSeries.Samples.Length; i++)
        {
            float weight = TimeSeries.GetWeight1byN1(i, 2f);
            float positionBlending = weight * UserControl;
            float directionBlending = weight * UserControl;
            Matrix4x4Extensions.SetPosition(ref GoalSeries.Transformations[i],
                Vector3.Lerp(GoalSeries.Transformations[i].GetPosition(), position, positionBlending));
            Matrix4x4Extensions.SetRotation(ref GoalSeries.Transformations[i],
                Quaternion.LookRotation(
                    Vector3.Slerp(GoalSeries.Transformations[i].GetForward(), direction, directionBlending),
                    Vector3.up));
        }

        //Actions
        for (int i = TimeSeries.Pivot; i < TimeSeries.Samples.Length; i++)
        {
            float w = (float) (i - TimeSeries.Pivot) / (float) (TimeSeries.FutureSampleCount);
            w = Utility.Normalise(w, 0f, 1f, 1f / TimeSeries.FutureKeyCount, 1f);
            for (int j = 0; j < GoalSeries.Actions.Length; j++)
            {
                float weight = GoalSeries.Values[i][j];
                weight = 2f * (0.5f - Mathf.Abs(weight - 0.5f));
                weight = Utility.Normalise(weight, 0f, 1f, UserControl, 1f - UserControl);
                if (actions[j] != GoalSeries.Values[i][j])
                {
                    GoalSeries.Values[i][j] = Mathf.Lerp(
                        GoalSeries.Values[i][j],
                        Mathf.Clamp(
                            GoalSeries.Values[i][j] +
                            weight * UserControl * Mathf.Sign(actions[j] - GoalSeries.Values[i][j]), 0f, 1f),
                        w);
                }
            }
        }
    }

    private void ApplyDynamicGoal(Matrix4x4 root, Vector3 move, float turn, float[] actions)
    {
        //Transformations
        Vector3[] positions_blend = new Vector3[TimeSeries.Samples.Length];
        Vector3[] directions_blend = new Vector3[TimeSeries.Samples.Length];
        float time = 2f;
        for (int i = 0; i < TimeSeries.Samples.Length; i++)
        {
            float weight = TimeSeries.GetWeight1byN1(i, 0.5f);
            float bias_pos = 1.0f - Mathf.Pow(1.0f - weight, 0.75f);
            float bias_dir = 1.0f - Mathf.Pow(1.0f - weight, 0.75f);
            directions_blend[i] = Quaternion.AngleAxis(bias_dir * turn, Vector3.up) *
                                  Vector3.ProjectOnPlane(root.GetForward(), Vector3.up).normalized;
            if (i == 0)
            {
                positions_blend[i] = root.GetPosition() +
                                     Vector3.Lerp(
                                         GoalSeries.Transformations[i + 1].GetPosition() -
                                         GoalSeries.Transformations[i].GetPosition(),
                                         time / (TimeSeries.Samples.Length - 1f) *
                                         (Quaternion.LookRotation(directions_blend[i], Vector3.up) * move),
                                         bias_pos
                                     );
            }
            else
            {
                positions_blend[i] = positions_blend[i - 1] +
                                     Vector3.Lerp(
                                         GoalSeries.Transformations[i].GetPosition() -
                                         GoalSeries.Transformations[i - 1].GetPosition(),
                                         time / (TimeSeries.Samples.Length - 1f) *
                                         (Quaternion.LookRotation(directions_blend[i], Vector3.up) * move),
                                         bias_pos
                                     );
            }
        }

        for (int i = 0; i < TimeSeries.Samples.Length; i++)
        {
            Matrix4x4Extensions.SetPosition(ref GoalSeries.Transformations[i],
                Vector3.Lerp(GoalSeries.Transformations[i].GetPosition(), positions_blend[i], UserControl));
            Matrix4x4Extensions.SetRotation(ref GoalSeries.Transformations[i],
                Quaternion.Slerp(GoalSeries.Transformations[i].GetRotation(),
                    Quaternion.LookRotation(directions_blend[i], Vector3.up), UserControl));
        }

        //Actions
        for (int i = TimeSeries.Pivot; i < TimeSeries.Samples.Length; i++)
        {
            float w = (float) (i - TimeSeries.Pivot) / (float) (TimeSeries.FutureSampleCount);
            w = Utility.Normalise(w, 0f, 1f, 1f / TimeSeries.FutureKeyCount, 1f);
            for (int j = 0; j < GoalSeries.Actions.Length; j++)
            {
                float weight = GoalSeries.Values[i][j];
                weight = 2f * (0.5f - Mathf.Abs(weight - 0.5f));
                weight = Utility.Normalise(weight, 0f, 1f, UserControl, 1f - UserControl);
                if (actions[j] != GoalSeries.Values[i][j])
                {
                    GoalSeries.Values[i][j] = Mathf.Lerp(
                        GoalSeries.Values[i][j],
                        Mathf.Clamp(
                            GoalSeries.Values[i][j] +
                            weight * UserControl * Mathf.Sign(actions[j] - GoalSeries.Values[i][j]), 0f, 1f),
                        w);
                }
            }
        }
    }
}